{
 "cells": [
  {
   "cell_type": "markdown",
   "source": [
    "## Определение позиций систем координат звеньев манипулятора и особых точек в базовой системе координат"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Импорты"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true,
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "import math\n",
    "from math import pi\n",
    "\n",
    "import numpy as np\n",
    "import sympy as sy\n",
    "\n",
    "from robobase.forwardkinematics import ForwardKinematics\n",
    "from robobase.dhmatrix import DHMatrix\n",
    "from src.support import toNumpy"
   ]
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Переменные звеньев (theta1, theta2, theta3, theta4, theta5)"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "outputs": [],
   "source": [
    "joint_coords = [math.pi, math.pi / 2, math.pi / 2, -math.pi / 2, -math.pi / 6]"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Параметры Денавита-Хартенберга манипулятора"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "outputs": [],
   "source": [
    "theta1, theta2, theta3, theta4, theta5 = sy.symbols('theta1'), sy.symbols('theta2'), \\\n",
    "                                         sy.symbols('theta3'), sy.symbols('theta4'),\\\n",
    "                                         sy.symbols('theta5')\n",
    "\n",
    "r = [0, 5.75, 7.375, 0, 0]\n",
    "alpha = [-pi / 2, 0, 0, -pi / 2, 0]\n",
    "d = [3, 0, 0, 0, 4.125]\n",
    "thetas = [theta1, theta2 - pi / 2, theta3 + pi / 2, theta4 - pi / 2, theta5]\n",
    "\n",
    "arg = {theta1: joint_coords[0], theta2: joint_coords[1], theta3: joint_coords[2],\n",
    "       theta4: joint_coords[3], theta5: joint_coords[4]}"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Особые точки, описанные относительно схвата манипулятора"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "##### Переменные точек"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "outputs": [],
   "source": [
    "e = 1.125\n",
    "g = 2"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "##### Координаты точек"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "outputs": [],
   "source": [
    "point1 = sy.Matrix([0, 0, -e, 1])\n",
    "point2 = sy.Matrix([g/2, 0, -e, 1])\n",
    "point3 = sy.Matrix([-g/2, 0, -e, 1])\n",
    "point4 = sy.Matrix([g / 2, 0, 0, 1])\n",
    "point5 = sy.Matrix([-g / 2, 0, 0, 1])\n",
    "\n",
    "points = [point1, point2, point3, point4, point5]"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Формирование всех матриц преобразования T(i-1, i)"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "outputs": [],
   "source": [
    "links = ForwardKinematics.links(d, r, thetas, alpha)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Формирование матриц преобразования T(0, i) где i от 0 до N"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%% md\n"
    }
   }
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "outputs": [],
   "source": [
    "frames = ForwardKinematics.frames(links)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Координаты звеньев манипулятора в базовой системе"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.0, 0.0, 0.0]\n",
      "[0.0, 0.0, 3.0]\n",
      "[-5.75, 0.0, 3.0]\n",
      "[1.625, -0.0, 3.0]\n",
      "[1.625, -0.0, 3.0]\n",
      "[1.625, -0.0, -1.125]\n"
     ]
    }
   ],
   "source": [
    "for frame in frames:\n",
    "    print(np.round(toNumpy(DHMatrix.pos(frame).evalf(subs=arg)), 3).T.tolist()[0])"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Результирующая матрицы преобразования в числовом виде"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "⎡ -0.87     -0.5    -1.5e-32    1.6   ⎤\n",
      "⎢                                     ⎥\n",
      "⎢ -0.5      0.87    -1.2e-16  -7.0e-16⎥\n",
      "⎢                                     ⎥\n",
      "⎢6.1e-17  -1.1e-16    -1.0      -1.1  ⎥\n",
      "⎢                                     ⎥\n",
      "⎣   0        0         0        1.0   ⎦\n"
     ]
    }
   ],
   "source": [
    "result_matrix = frames[-1].evalf(subs=arg)\n",
    "\n",
    "sy.pprint(result_matrix .evalf(2))"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Точки массива points в базовой системе"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[1.625, -0.0, -0.0]\n",
      "[0.759, -0.5, -0.0]\n",
      "[2.491, 0.5, -0.0]\n",
      "[0.759, -0.5, -1.125]\n",
      "[2.491, 0.5, -1.125]\n"
     ]
    }
   ],
   "source": [
    "for point in points:\n",
    "    print(np.round(toNumpy(result_matrix  * point), 3).T.tolist()[0][:-1])"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}